/*
The surface-physics project: http:// code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0  

Bump_MeshCollider.cpp
Bump_MeshCollider implementation.


*/


#include "Bump_MeshCollider.h"



Registrar Bump_MeshCollider::registrar("MeshCollider", Bump_MeshCollider::create);


/**
 * Constructor for this derived class
 */
Bump_MeshCollider::Bump_MeshCollider():
		ground(NULL),
		colliderState(CS_UNINITIALIZED),
		collisionCount(0),		
		MAX_COLLISIONS(5),		
		kinematicBodyActive(false),
		dynamicVehicleActive(false),
		collisionMeshActive(false),
		hoverEnginesFiredForFlight(false)
{
	deleteIfInactive = false;
	strcpy(className, "VesselCollider");

	// Vehicle
	rbCollider = NULL;
	vehicle = NULL;
	frontWheelShape = 0;
	mass = 20000.f;


	// Engine parameters
	maxEngineForce = 3000.f;// this should be engine/velocity dependent
	maxBrakingForce = 500.f;
	gBrakingForce = 0.f;
	gVehicleSteering = 0.f;
	gEngineForce = 0.f;  // initial force
	reverseEngineForce = -maxEngineForce/4;

	// Transmission : simple gear model which varies engine force only
	maxGear = 6; // Reverse and neutral indicated by 'R' & 'N'
	minGear = 1;
	m_gear = 1;

	// Steering parameters
	steeringIncrement = 0.04f;
	steeringClamp = 0.3f;

	/**
	 * Wheel parameters : Wheels continue to penetrate triangle mesh, not because of physics model,
	 * but because wheel mesh and physics wheel not aligned(physics wheel is more inwards,
	 * collision mesh wheel more outwards : there is actually no penetration)
	 */
	frontWheelRadius = 0.24;
	backWheelRadius = 0.5f;
	wheelHalfWidth = 0.2f;	 // actual width is 2.364(twice)
	wheelFriction = 1500;
	wheelFwdBackHalfDist = 2.7f;
    wheelLeftRightHalfDist = 1.3f;
	connectionHeight = 0.f;
	rightIndex = 0;
	upIndex = 1;
	forwardIndex = 2;
	wheelDirectionCS0.setValue(0,-1,0);
	wheelAxleCS.setValue(-1,0,0);

	// Suspension parameters
	suspensionStiffness = 70.f;
	suspensionDamping = 50.0f;
	suspensionCompression = 0.f;
	rollInfluence = 0.08f;// 1.0f;
	suspensionRestLength = 2.2f;

	// Heights and initial location

	// THIS MUST BE 0 - Otherwise kinematic vehicle wont report collisions as wheels wont be
	// touching ground - its not a vehicle deactivation issue AT ALL !!
	initialVehicleHeight = 0.;

	chassisHeight = 0.; //1.2; // Height of chassis above vehicle origin - unused
	chassisHalfLength = 9.5f;
	chassisHalfWidth = 8.5f;
	chassisHalfHeight = 1.f;

	linearVel  = btVector3(0.f, 0.f, 0.f);
	angularVel = btVector3(0.f, 0.f, 0.f);

	collisionInfo.colliding = false;
	collisionInfo.thisObject = this;
}


/**
 * Destructor, does nothing currently
 */
Bump_MeshCollider::~Bump_MeshCollider()
{
	oapiWriteLogV("Bump_MeshCollider::~Bump_MeshCollider: Destroying %s", strObjectName);

	// If a collider is active and sim is ending then we must destroy the physics object now
	if(colliderState == CS_ORBITER_REGIME || colliderState == CS_BULLET_REGIME) {

		int rv = destroyBulletRigidBodyVehicle();
		if(rv){
			oapiWriteLogV("Bump_MeshCollider::~Bump_MeshCollider: ERROR : Destroying vehicle failed %s",
					strObjectName);
		}

		if(!Bump_BulletBase::simIsEnding && collisionMeshActive) {

			// Set the collision mesh
			for (int i=0; i < vehicle->getNumWheels(); i++)
				collMshWheel[i].removeShape();

			collMshChassis.removeShape();
			collisionMeshActive = false;
		}
	}


	// Kept around till the end so it does not have to be recreated everytime state changes
	// Both kinematic and dynamic vehicle share this
	delete ground;
}


/**
 * Initialize a base attachment.
 * Creates a new BulletBase Attachment and inserts it in the Orbiter world & Bullet World
 *
 * The search or create functionality provided here has to be part of this derived class as
 * for the other derived class, there is no search required(Meshes must be created always)
 *
 * A attachment needs to be added to the BulletBase. There can be at most MAX_OBJECTS number of attachments
 * The check for whether the number of attachments is < MAX_OBJECTS is already done at
 * Bump_BulletBase::readCreatePhysicsFileObject()
 *
 * Also a single attachment point needs to be created in the vessel that will be attached.
 *
 * Both attachment points may need to be deleted midway(use Orbiter's functions)
 *
 *
 *
 * TODO: Currently only searches and attaches, no create.
 * TODO: Check if vessel attachment point already exists, also remove attachment point whenever detached
 *
 */
int
Bump_MeshCollider::clbkSetClassCaps()
{
	int rv;
	

	oapiWriteLogV("Bump_MeshCollider::init: Initializing %s", strObjectName);


	// If the object ref is valid go ahead and get the VESSEL ptr, its needed BEFORE attaching
	if(hObjThis){

		vCollider = (VESSEL3*)oapiGetVesselInterface(hObjThis);
		vCollider->GetEquPos(lng, lat, rad);
		

		mass = (btScalar)vCollider->GetMass();
		vesselRadius = (btScalar)vCollider->GetSize();
	}
	else{
		oapiWriteLogV("Bump_MeshCollider::init: ERROR :The hObj passed (%p) wasn't valid for %s",
				hObjThis, strObjectName);
		return 1;
	}


	// Read the configuration for this orbiter vessel class from physics file
	rv = readPhysicsFile();
	if(rv){
		oapiWriteLogV("Bump_MeshCollider::init: ERROR : Reading options from physics file failed %s", strObjectName);
		return 2;
	}

	// Choose the proper terrain generator as picked by the base.
	switch(bumpBase->terrainGenerator){
	case TT_PLANETCURVE:
		ground = new Bump_TerrainPatch(16, 16, 0.000002f, 0.000002f, 0.000010f, 0.000010f, 10.f, bumpBase);
		break;
	case TT_ORULEX:
		ground = new Bump_TerrainPatchOru(16, 16, 0.000002f, 0.000002f, 0.000010f, 0.000010f, 10.f, bumpBase);
		break;
	case TT_D3D11:
		break;
	default:
		oapiWriteLogV("Bump_MeshCollider::init: ERROR : There is no terrain generator specified for %s !!", strObjectName);
		// No terrain!!, physics controlled objects will fall through, should never happen !
		break;
	}

	if(!ground){
		oapiWriteLogV("Bump_MeshCollider::init: ERROR : Could not add ground for %s", strObjectName);
		return 3;
	}

	// Create terrain patch from initial equatorial position of vessel
	rv = ground->createPatch(lng, lat);
	if(rv){
		oapiWriteLogV("Bump_MeshCollider::init: ERROR : Could not add ground for %s", strObjectName);
		return 4;
	}




	// Attachment and Bullet body creation will happen later, till then physicsInitialized = false


	return 0;
}


/**
 * Read collision mesh from physics file for this vessel
 * This may be needed only for reading a particular ship's collision mesh
 */
int
Bump_MeshCollider::readPhysicsFile(void)
{
	return 0;

}


/**
 * Updates a base attachment
 * Primarily move the vessel mesh into position according to latest transform by setting
 * attachment point parameter.
 * Needs to read and process any commands too from the containing base
 */
int
Bump_MeshCollider::clbkPostStep(double SimT, double SimDT, double MJD)
{

	// oapiWriteLogV("Bump_MeshCollider::poststep: The next sim step will advance the sim by %f secs", SimDT);

	int rv = 0;

	switch(colliderState){

		// This state is important for deleting this object and also for raising landed ships
		// above terrain(Orbiter puts them on the planet surface !)
		case CS_UNINITIALIZED:
		{
			double lng, lat, rad;			

			// Get vessel position in both co-ord systems
			vCollider->GetEquPos(lng, lat, rad);

			// Get exact distance to Touch down points(not CoG) which = rad - cogElev
			cogElev = vCollider->GetCOG_elev();
			rad -= cogElev;

			// delete this collider if vessel beyond max distance, else we can proceed with attaching
			// this may need more work
			if(rad > bumpBase->maxTerrainRadialDist)
				deleteMe = true;
			else
				colliderState = CS_ORBITER_REGIME;

			// sprintf(oapiDebugString(), "CS_UNINITIALIZED; rad : %f ", rad);


			break;
		}



		// --------------------------- Orbiter Regime -----------------------------
		// Transfers Orbiter vehicle state to kinematic object continuously
		// Used to push away any colliding objects
		// Any collision causes a transition to the next state and the kinematic object to be replaced
		// with a dynamic rigid body
		case CS_ORBITER_REGIME:
		{
			btScalar m[16];

			// Insert the physics kinematic body, no need to remove vehicle, it should be already gone!
			// removed in BULLET_REGIME itself when leaving that state
			if(!kinematicBodyActive){

				// TODO: Must check here if gear up/down
				rv = createBulletRigidBodyVehicle(true);
				if(rv){
					oapiWriteLogV("Bump_MeshCollider::poststep: ERROR : Creating KINEMATIC vehicle failed %s", strObjectName);
					return 1;
				}

				kinematicBodyActive = true;
			}

			if(oapiGetFocusObject() == hObjThis)
				PhysicsEngine::focusBody = rbCollider;

			btTransform transVessel;
			getVesselTransformInBaseFrame(transVessel);
			rbCollider->getMotionState()->setWorldTransform(transVessel);


			// Get velocities
			linearVel  = rbCollider->getLinearVelocity();
			angularVel = rbCollider->getAngularVelocity();


			// Get exact distance to Touch down points(not CoG) which = rad - cogElev
			// cogElev can change based on gear up/down
			vCollider->GetEquPos(lng, lat, rad);
			cogElev = vCollider->GetCOG_elev();
			rad -= cogElev;

			//  Now check if the wheels are colliding, OR with chassis collision info from nearphase
			//  Note: that m[] will be changed below
			for (int i=0;i<vehicle->getNumWheels();i++) {

				collisionInfo.colliding |=vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact;

				// synchronize the wheels with the (interpolated) chassis worldtransform
				vehicle->updateWheelTransform(i, true);

				btTransform transWheel = vehicle->getWheelInfo(i).m_worldTransform;
				transWheel.getOpenGLMatrix(m);

				btVector3 trueWheelPos = btVector3(m[12], m[13], m[14]) - bumpBase->simBoxOffset;

				// Place collision meshes in Orbiter window - DO NOT MOVE TO END of case
				// collisionInfo updated in this loop & must be right here
				btTransform transShape = transWheel;
				transShape.setOrigin(trueWheelPos);
				collMshWheel[i].drawShape(transShape);
			}


			btVector3 knmOrigin = transVessel.getOrigin();
			sprintf(oapiDebugString(), "CS_ORBITER_REGIME; %f %f, %f, rad:%f"
					"count=%d, colliding:%d", BT3ARGS(knmOrigin), collisionCount, collisionInfo.colliding, rad);


			// Do not attach yet
			// If physics engine signals a collision move to BULLET_REGIME and attach(take over control)
			if(collisionInfo.colliding){
				++collisionCount;
				if(collisionCount == MAX_COLLISIONS){

					// Need to destroy and remove kinematic vehicle completely
					rv = destroyBulletRigidBodyVehicle();
					if(rv){
						oapiWriteLogV("Bump_MeshCollider::poststep: ERROR : Destroying KINEMATIC vehicle failed %s", strObjectName);
						return 2;
					}

					kinematicBodyActive = false;
					collisionCount = 0;

					colliderState = CS_PRE_BULLET_REGIME;
				}
			}
			else if(rad > bumpBase->maxTerrainRadialDist){

				// Destroy kinematic mesh
				rv = destroyBulletRigidBodyVehicle();
				if(rv){
					oapiWriteLogV("Bump_MeshCollider::poststep: ERROR : Destroying KINEMATIC vehicle failed %s", strObjectName);
					return 3;
				}

				kinematicBodyActive = false;
				colliderState = CS_UNINITIALIZED;   // this move back to CS_UNINITIALIZED may not be needed !
			}


			// Update terrain - for detecting terrain collision in next iteration
			if(ground)
				ground->updatePatch(lng, lat, rad);

			// Draw the chassis collision mesh - TODO: check if display is enabled
			btTransform transFinal = transVessel;
			transFinal.setOrigin(transVessel.getOrigin() - bumpBase->simBoxOffset); // this needs validation
			collMshChassis.drawShape(transFinal);

			break;
		}


		// Activate the vessel in the Orbiter simulation by firing the hover thrusters
		case CS_PRE_BULLET_REGIME:
		{
			DWORD d = vCollider->GetFlightStatus();

			if (d == 0) {
				if (hoverEnginesFiredForFlight) {
					// Shut them
					vCollider->SetThrusterGroupLevel(vCollider->GetThrusterGroupHandle(THGROUP_HOVER), 0.f);
					hoverEnginesFiredForFlight = false;
				}

				colliderState = CS_BULLET_REGIME;
			}
			else {
				vCollider->SetThrusterGroupLevel(vCollider->GetThrusterGroupHandle(THGROUP_HOVER), 0.1f);
				hoverEnginesFiredForFlight = true;
			}

			sprintf(oapiDebugString(), "CS_PRE_BULLET_REGIME : hoverEnginesFiredForFlight:%d", hoverEnginesFiredForFlight);

			break;
		}



		// ------------------------------Bullet Regime -----------------------------------
		// This state does not apply any torques as that would imply more accurate MOI
		// Tradeoff : Torques have a negligible effect for
		// getting collision response. Thrusters are applied in Bullet. Any thruster torques
		// to avoid collision will be ineffective once objects have collided.
		case CS_BULLET_REGIME:
		{

			if(!dynamicVehicleActive){

				// Replace the kinematic body from ORBITER_REGIME with a vehicle to allow terrain/deck roll
				rv = createBulletRigidBodyVehicle(false);
				if(rv){
					oapiWriteLogV("Bump_MeshCollider::poststep: ERROR : Creating DYNAMIC vehicle failed %s", strObjectName);
					return 5;
				}

				dynamicVehicleActive = true;
			}

			if(oapiGetFocusObject() == hObjThis)
				PhysicsEngine::focusBody = rbCollider;


			// Reverse direction of control now : Bullet to Orbiter

			// Main world transform of the vehicle(in base frame)
			btScalar m[16];
			btTransform transVessel = rbCollider->getCenterOfMassTransform();
			transVessel.getOpenGLMatrix(m);

			//  trueChassisPos is persisted as a class member & passed to transformVessel()
			trueChassisPos = btVector3(m[12], m[13],	m[14]) - bumpBase->simBoxOffset;
			transformVessel();			

			sprintf(oapiDebugString(), "B : %f, %f, %f", BT3ARGS(trueChassisPos));


			// Here we work in vessel frame only
			VECTOR3 vdirWeight, vdirUnitWeight, vdirLift, vdirThrust, vdirDrag, upThrust;

			// TODO: Local vessel gravity must be used inside ships with artifical gravity
			vCollider->GetWeightVector(vdirWeight);
			vdirUnitWeight = unit(vdirWeight);
			double magnWeight = dotp(vdirWeight, vdirUnitWeight);

			vCollider->GetLiftVector(vdirLift);
			vCollider->GetThrustVector(vdirThrust);
			vCollider->GetDragVector(vdirDrag);

			upThrust = vdirLift + vdirThrust + vdirDrag;
			double magnUpThrust =  dotp(upThrust, -vdirUnitWeight);

			// TODO: Apply torque vector as GetTorque() will work now !!

			// For next iteration
			VECTOR3 fWRTVessel = upThrust/10., f;

			// Convert vector to base frame
			convertVesselVec2BaseVec(fWRTVessel, f);

			rbCollider->applyCentralForce(btVector3((btScalar)f.x, (btScalar)f.y, (btScalar)f.z));
			rbCollider->activate(true); //This is needed - do not remove, vehicle is de-activated otherwise

			// Now check if the wheels are colliding, OR with chassis collision info from nearphase
			for (int i=0;i<vehicle->getNumWheels();i++) {
				collisionInfo.colliding |= vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact;
				vehicle->setBrake(2.0, i);
			}

			// Get exact distance to Touch down points(not CoG) which = rad - cogElev
			// cogElev can change based on gear up/down
			bumpBase->bb->Local2Global(_V(m[12], m[13], m[14]), gposVesselPos);
			oapiGlobalToEqu(bumpBase->hObjRefBody, gposVesselPos, &lng, &lat, &rad);
			cogElev = vCollider->GetCOG_elev();
			rad -= cogElev;

			// We move out of Bullet control if by any chance vessel is flung too far out
			double distanceAboveTerrain = getDistanceAboveTerrain(rad);

			// Transition to Orbiter only when enough thrust and no collisions happening
			if( (magnUpThrust > magnWeight || distanceAboveTerrain > 100.f) && !collisionInfo.colliding){

				// Replace the kinematic vehicle from ORBITER_REGIME with a vehicle to allow terrain/deck roll
				rv = destroyBulletRigidBodyVehicle();
				if(rv){
					oapiWriteLogV("Bump_MeshCollider::poststep: ERROR : Destroying DYNAMIC vehicle failed %s", strObjectName);
					return 8;
				}

				dynamicVehicleActive = false;
				colliderState = CS_ORBITER_REGIME;   // this move back to CS_UNINITIALIZED may not be needed !

			}
			

			// Update terrain to allow vessel collisions on it.
			// We must convert the physics position to equ pos and not use the lolarad from
			// vCollider->GetEquPos(lng, lat, rad) directly, as that stays fixed as long as vessel is
			// attached ! So vessel wont move if we directly use it. The physics position does change.
			if(ground){
				bumpBase->bb->Local2Global(_V(m[12], m[13], m[14]), gposVesselPos);
				oapiGlobalToEqu(bumpBase->hObjRefBody, gposVesselPos, &lng, &lat, &rad);

				// oapiWriteLogV("Bump_MeshCollider::poststep: Converted (%f,%f,%f) to lolara(%f,%f,%f)",
				// 		m[12], m[13], m[14], lng, lat, rad);

				ground->updatePatch(lng, lat, rad);
			}

			//  Now for the wheels - this is done last as wheels need not be updated if not needed
			//  & m[] must not be changed before gposVesselPos is determined
			for (int i=0;i<vehicle->getNumWheels();i++){

				// synchronize the wheels with the (interpolated) chassis worldtransform
				vehicle->updateWheelTransform(i, true);

				btTransform transWheel = vehicle->getWheelInfo(i).m_worldTransform;
				transWheel.getOpenGLMatrix(m);

				btVector3 trueWheelPos = btVector3(m[12], m[13],	m[14]) - bumpBase->simBoxOffset;

				//  Place collision meshes in Orbiter window
				btTransform transShape = transWheel;
				transShape.setOrigin(trueWheelPos);
				collMshWheel[i].drawShape(transShape);
			}

			// Show collision mesh
			btTransform transFinal = transVessel;
			transFinal.setOrigin(trueChassisPos + transLocalChassis.getOrigin());
			collMshChassis.drawShape(transFinal);


		/*	sprintf(oapiDebugString(), "CS_BULLET_REGIME : magnUpThrust:%f,  magnWeight:%f, colliding=%d, rad:%f",
								magnUpThrust, magnWeight, collisionInfo.colliding, rad);*/


			break;
		}


		case CS_FROZEN:
		{
			// Freezing not implemented yet, maybe just freeze the base
			break;
		}


		default:
			break;
	} // end switch()


	// Prepare for next iteration's collision detection
	collisionInfo.colliding = false;

	return 0;
}


/**
 *	Process keyboard input any other messages
 */
int
Bump_MeshCollider::processCommand()
{
	return 0;
}


/**
 * Transfer Bullet rigid body state :
 * > Position : same as attachment point release, so not needed to be transferred
 * > Orientation : transferred by the attachment point detachment angle
 * > Linear Velocity : yes, absolutely needed, to prevent a pause before Orbiter engine kicks in !
 * > Rotational Velocity : yes
 */
int
Bump_MeshCollider::transformVessel()
{
	VECTOR3 rposVesselWRTRefBody;
	VECTOR3 rvelBaseWRTRefBody, rposBaseWRTRefBody, rvelVesselWRTRefBody;
	VECTOR3 ravelVesselWRTBase;

	// Position
	bumpBase->bb->Local2Rel(_V(BT3ARGS(trueChassisPos)), rposVesselWRTRefBody);


	// Orientation
	btTransform transCollider = rbCollider->getCenterOfMassTransform();
	btMatrix3x3 mBt = transCollider.getBasis();

	btVector3 X = mBt.getRow(0);
	btVector3 Y = mBt.getRow(1);
	btVector3 Z = mBt.getRow(2);

	MATRIX3 mOrb = { BT3ARGS(X),
			 	 	 BT3ARGS(Y),
					 BT3ARGS(Z),
					};

	MATRIX3 bbRot;

	bumpBase->bb->GetRotationMatrix(bbRot);

	MATRIX3 final = mul(bbRot, mOrb);


	vCollider->SetRotationMatrix(final);

	// sprintf(oapiDebugString(), "CS_BULLET_REGIME : rx:%f, ry:%f, rz:%f", rx*DEG, ry*DEG, rz*DEG);

	// Linear velocity
	btVector3 rvelBtVessel = rbCollider->getLinearVelocity();   // base frame
	bumpBase->bb->Local2Rel( _V(BT3ARGS(rvelBtVessel)), rvelVesselWRTRefBody);

	bumpBase->bb->Local2Rel(_V(0., 0., 0.), rposBaseWRTRefBody);
	bumpBase->bb->GetRelativeVel(bumpBase->hObjRefBody, rvelBaseWRTRefBody); // this is not 0

	rvelVesselWRTRefBody -= rposBaseWRTRefBody;  // to compensate for Local2Rel() converting a direction vector
	rvelVesselWRTRefBody += rvelBaseWRTRefBody;  // to account for orbital speed of base(rotating with the Moon)


	// Angular velocity
	btVector3 ravelBtVessel = rbCollider->getAngularVelocity();   // base frame
	ravelVesselWRTBase = _V(-ravelBtVessel.x(), -ravelBtVessel.y(), -ravelBtVessel.z());




	// Vessel status stuff
	VESSELSTATUS2 vs;
	memset(&vs, 0, sizeof(VESSELSTATUS2));
	vs.version = 2;
	vCollider->GetStatusEx(&vs);

	VECTOR3 ang = vs.arot;

	// sprintf(oapiDebugString(), "CS_BULLET_REGIME : rx:%f, ry:%f, rz:%f", rx*DEG, ry*DEG, rz*DEG);

	vs.rpos = rposVesselWRTRefBody;

	vs.rvel = rvelVesselWRTRefBody;
	vs.vrot = ravelVesselWRTBase;

	vCollider->DefSetStateEx(&vs);

	return 0;
}


/**
 * Create the physics counterpart in the Bullet world
 *
 */
int
Bump_MeshCollider::createBulletRigidBodyVehicle(bool isKinematic)
{

	bool isFrontWheel;
	char meshName[MAX_STRLEN];


	// The compound shape for the chassis
	compoundChassisShape = new btCompoundShape();

	// The chassis shape to be added to the compound shape
	chassisBoxShape = new btBoxShape(btVector3(chassisHalfWidth,
											   chassisHalfHeight,
											   chassisHalfLength));

	// localTrans for the chassis shape
	// It effectively shifts the center of mass with respect to the chassis
	transLocalChassis.setIdentity();
	transLocalChassis.setOrigin(btVector3(0, chassisHeight, 1.0));

	// Add chassis shape to compound shape
	compoundChassisShape->addChildShape(transLocalChassis, chassisBoxShape);

	btTransform transVessel;
	getVesselTransformInBaseFrame(transVessel);
	mass = (btScalar)vCollider->GetMass();
	
	btVector3 v = transVessel.getOrigin();
	oapiWriteLogV("%f, %f, %f", BT3ARGS(v));

	// The vehicle action requires a non-zero mass for a kinematic body too
	if(isKinematic)
		rbCollider = phyEngine->addKinematicBody(1.0f, transVessel, compoundChassisShape);
	else{
		btVector3 aabbMinGround, aabbMaxGround;
		ground->rbTerrainPatch->getAabb(aabbMinGround, aabbMaxGround);
		// oapiWriteLogV("rbCollider: %f, grnd:%f", aabbMin.y(), aabbMaxGround.y());

		// If collider AABB below ground, it will get stuck, it must be raised
		if(aabbMin.y() <  aabbMaxGround.y()){
			oapiWriteLog("Collider may get STUCK");
			btVector3 bsphCenter;
			btScalar bsphRad;
			chassisBoxShape->getBoundingSphere(bsphCenter, bsphRad);
			transVessel.setOrigin(btVector3(v.x(), aabbMaxGround.y() + bsphRad, v.z()));

			v = transVessel.getOrigin();
			oapiWriteLogV("STUCK FIX : %f, %f, %f", BT3ARGS(v));
		}


		rbCollider = phyEngine->addRigidBodyMS(mass/10.f, transVessel, compoundChassisShape);
	}

	// Set initial velocities
	rbCollider->setLinearVelocity(linearVel);
	rbCollider->setAngularVelocity(angularVel);

	rbCollider->setActivationState(DISABLE_DEACTIVATION);


	// Collision flag
	rbCollider->setUserPointer(&collisionInfo);


	// Now the wheels, first the cylinder shape for the wheels
	frontWheelShape = new btCylinderShapeX(btVector3(wheelHalfWidth,frontWheelRadius, frontWheelRadius));
	backWheelShape = new btCylinderShapeX(btVector3(wheelHalfWidth, backWheelRadius, backWheelRadius));


	// Create vehicle
	vehicleRayCaster = new btDefaultVehicleRaycaster(bumpBase->m_dynamicsWorld);
	vehicle = new btRaycastVehicle(wheelTuning, rbCollider, vehicleRayCaster);

	bumpBase->m_dynamicsWorld->addVehicle(vehicle);

	// Get TDPs in vessel frame
	VECTOR3 pt1, pt2, pt3;
	vCollider->GetTouchdownPoints(pt1, pt2, pt3);

	// choose coordinate system
	vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);

	// Forward wheel
	isFrontWheel=true;
	btVector3 connectionPoint = btVector3((btScalar)pt1.x, connectionHeight, (btScalar)pt1.z /*- 1.0f*/);
	vehicle->addWheel(connectionPoint,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,frontWheelRadius,wheelTuning,isFrontWheel);

	isFrontWheel = false;
	// backward left wheel
	connectionPoint = btVector3((btScalar)pt2.x, connectionHeight + 0.1f, (btScalar)pt2.z);
	vehicle->addWheel(connectionPoint,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,backWheelRadius,wheelTuning,isFrontWheel);

	// backward right wheel
	connectionPoint = btVector3((btScalar)pt3.x, connectionHeight + 0.1f, (btScalar)pt3.z);
	vehicle->addWheel(connectionPoint,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,backWheelRadius,wheelTuning,isFrontWheel);

	// Set wheel properties for all 4 wheels
	for (int i=0;i<vehicle->getNumWheels();i++){
		btWheelInfo& wheel = vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = suspensionStiffness;
		wheel.m_wheelsDampingRelaxation = suspensionDamping;
		wheel.m_wheelsDampingCompression = suspensionCompression;
		wheel.m_frictionSlip = wheelFriction;
		wheel.m_rollInfluence = rollInfluence;
	}

	// Celestial body specific gravity
	rbCollider->setGravity(btVector3((btScalar)bumpBase->G.x, (btScalar)bumpBase->G.y, (btScalar)bumpBase->G.z));


	// rbCollider->setGravity(btVector3(0., -10, 0.));


	// Wheel rendering
	m_ShapeDrawer = phyEngine->getShapeDrawer();
	phyEngine->addExternalRenderObject(this, renderFuncPos);



	if(!collisionMeshActive) {

		// Set the collision mesh

		sprintf(meshName, "%s_Wheel%d", strObjectName, 0);
		collMshWheel[0].addShape(bumpBase, frontWheelShape, meshName);

		sprintf(meshName, "%s_Wheel%d", strObjectName, 1);
		collMshWheel[1].addShape(bumpBase, backWheelShape, meshName);

		sprintf(meshName, "%s_Wheel%d", strObjectName, 2);
		collMshWheel[2].addShape(bumpBase, backWheelShape, meshName);



		sprintf(meshName, "%s_Chassis", strObjectName);
		btVector3 halfExtent = (static_cast<const btBoxShape*>(chassisBoxShape))->getHalfExtentsWithoutMargin();
		oapiWriteLogV("Halves in cpb : %f, %f, %f", BT3ARGS(halfExtent));
		collMshChassis.addShape(bumpBase, chassisBoxShape, meshName);
		collisionMeshActive = true;

		vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt1);
		vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt2);
		vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt3);

		oapiWriteLogV("TDP: (%f, %f, %f), (%f, %f, %f), (%f, %f, %f)",
				V3ARGS(pt1), V3ARGS(pt2), V3ARGS(pt3));
	}

	return 0;
}

/*
 * Wheels rendering for the vehicle
 */
int
Bump_MeshCollider::clbkPhysicsRender(void)
{
	int i;
	btScalar m[16];
	btVector3 wheelColor(1,0,0);
	btVector3	worldBoundsMin,worldBoundsMax;
	bumpBase->m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);


	for (i=0;i<vehicle->getNumWheels();i++)
	{
		// synchronize the wheels with the (interpolated) chassis worldtransform
		vehicle->updateWheelTransform(i,true);
		// draw wheels (cylinders)
		vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
		if(i==0)
			m_ShapeDrawer->drawOpenGL(m, frontWheelShape,wheelColor, phyEngine->getDebugMode(),worldBoundsMin,worldBoundsMax);
		else
			m_ShapeDrawer->drawOpenGL(m, backWheelShape,wheelColor, phyEngine->getDebugMode(),worldBoundsMin,worldBoundsMax);

	}

	return 0;
}


/**
 * De-allocates memory being consumed by the raycast vehicle so it can be
 * created again by createBulletRigidBody()
 *
 * This is a very important function. If memory is not freed up, Orbiter sim will not
 * exit properly and the Launchpad will not appear leading to user frustration!
 *
 * Here we de-allocate only the sub class specific stuff which have not been freed by
 * phyEngine.clientResetScene(), which is called before Bump_BulletBase::exit();
 *
 * Thus rigid bodies(derived from CollisionObjects), motion states are already removed
 * by the time we come here, DO NOT remove them again !
 *
 * The stuff which should be removed are any sub class specific classes used to model
 * this object like m_vehicleRayCaster & vehicle. Also any collision shapes not pushed
 * into m_collisionShapes[] during init(), needs to be freed here, such as the frontWheelShape
 */
int
Bump_MeshCollider::destroyBulletRigidBodyVehicle()
{

	rbCollider->getAabb(aabbMin, aabbMax);

	bumpBase->setBaseCameraFocus();

	// Remove this registered object from rendering list, as it will be destroyed after this
	phyEngine->removeExternalRenderObject(renderFuncPos);

	// THIS IS REQUIRED FOR VEHICLES FIRST
	bumpBase->m_dynamicsWorld->removeVehicle(vehicle);

	// Manually delete the collision shape instead of adding it to the sim wide list
	if(rbCollider && rbCollider->getMotionState())
	{
		delete rbCollider->getMotionState();
	}

	btCollisionObject *obj = dynamic_cast<btCollisionObject*>(rbCollider);
	bumpBase->m_dynamicsWorld->removeCollisionObject(obj);
	delete obj;


	// Vehicle specific objects
	delete vehicleRayCaster;
	delete vehicle;

	// These shapes were not added to m_collisionShapes[], so they have to be removed here
	// They were not added so that they could be specifically delete during reset()
	delete chassisBoxShape;
	delete compoundChassisShape;
	delete backWheelShape;
	delete frontWheelShape;

	//  Delete collision shape
	//  TODO : check if they exist



	// for (int i=0;i<vehicle->getNumWheels();i++)
		// delete collMshWheel[i];

	// delete ground;



	return 0;
}


